% function [plan_x,plan_y,yaw,cur,Points_Num,a0,a1,a2,a3,a4,a5]= fcn(start_x,start_y,rad_start_yaw,start_radius,end_x,end_y,rad_end_yaw,end_radius)
function [mm_plan_x,mm_plan_y,rad_yaw,pm_cur,Points_Num]= VtPolynoimal(mm_start_x,mm_start_y,rad_start_yaw,mm_start_radius,mm_end_x,mm_end_y,rad_end_yaw,mm_end_radius)

% start_x   __________________uint is mm
% start_y    __________________uint is mm
% rad_start_yaw    __________________uint is rad
% start_radius  __________________uint is mm

start_x=double(mm_start_x/1000);
start_y=double(mm_start_y/1000);
start_radius=double(mm_start_radius/1000);
rad_start_yaw=double(rad_start_yaw);

end_x=double(mm_end_x/1000);
end_y=double(mm_end_y/1000);
end_radius=double(mm_end_radius/1000);
rad_end_yaw=double(rad_end_yaw);

k1 = tan(rad_start_yaw);       %y'
k2 = tan(rad_end_yaw);        %y'
a = ((1+(k1)^2)^1.5)/start_radius;     %y''
b = -((1+(k2)^2)^1.5)/end_radius;    %y''

xa=start_x;
xb=end_x;
ya=start_y;
yb=end_y;

% 五次多项式系数 x1f、y1f为初始位置横、纵坐标；x2f、y2f为终止位置横、纵坐标
a0 = (2*xa^5*yb - 2*xb^5*ya + 2*k1*xa*xb^5 - 2*k2*xa^5*xb + 10*xa*xb^4*ya - 10*xa^4*xb*yb - a*xa^2*xb^5 + 2*a*xa^3*xb^4 - a*xa^4*xb^3 + b*xa^3*xb^4 - 2*b*xa^4*xb^3 + b*xa^5*xb^2 - 10*k1*xa^2*xb^4 + 8*k1*xa^3*xb^3 - 8*k2*xa^3*xb^3 + 10*k2*xa^4*xb^2 - 20*xa^2*xb^3*ya + 20*xa^3*xb^2*yb)/(2*(xa - xb)^2*(xa^3 - 3*xa^2*xb + 3*xa*xb^2 - xb^3));
a1 = (2*k2*xa^5 - 2*k1*xb^5 + 2*a*xa*xb^5 - 2*b*xa^5*xb + 10*k1*xa*xb^4 - 10*k2*xa^4*xb - a*xa^2*xb^4 - 4*a*xa^3*xb^3 + 3*a*xa^4*xb^2 - 3*b*xa^2*xb^4 + 4*b*xa^3*xb^3 + b*xa^4*xb^2 + 16*k1*xa^2*xb^3 - 24*k1*xa^3*xb^2 + 24*k2*xa^2*xb^3 - 16*k2*xa^3*xb^2 + 60*xa^2*xb^2*ya - 60*xa^2*xb^2*yb)/(2*(xa - xb)^2*(xa^3 - 3*xa^2*xb + 3*xa*xb^2 - xb^3));
a2 = -(a*xb^5 - b*xa^5 + 4*a*xa*xb^4 + 3*a*xa^4*xb - 3*b*xa*xb^4 - 4*b*xa^4*xb + 36*k1*xa*xb^3 - 24*k1*xa^3*xb + 24*k2*xa*xb^3 - 36*k2*xa^3*xb + 60*xa*xb^2*ya + 60*xa^2*xb*ya - 60*xa*xb^2*yb - 60*xa^2*xb*yb - 8*a*xa^2*xb^3 + 8*b*xa^3*xb^2 - 12*k1*xa^2*xb^2 + 12*k2*xa^2*xb^2)/(2*(xa - xb)^2*(xa^3 - 3*xa^2*xb + 3*xa*xb^2 - xb^3));
a3 = (a*xa^4 + 3*a*xb^4 - 3*b*xa^4 - b*xb^4 - 8*k1*xa^3 - 12*k2*xa^3 + 12*k1*xb^3 + 8*k2*xb^3 + 20*xa^2*ya - 20*xa^2*yb + 20*xb^2*ya - 20*xb^2*yb + 4*a*xa^3*xb - 4*b*xa*xb^3 + 28*k1*xa*xb^2 - 32*k1*xa^2*xb + 32*k2*xa*xb^2 - 28*k2*xa^2*xb - 8*a*xa^2*xb^2 + 8*b*xa^2*xb^2 + 80*xa*xb*ya - 80*xa*xb*yb)/(2*(xa^2 - 2*xa*xb + xb^2)*(xa^3 - 3*xa^2*xb + 3*xa*xb^2 - xb^3));
a4 = -(30*xa*ya - 30*xa*yb + 30*xb*ya - 30*xb*yb + 2*a*xa^3 + 3*a*xb^3 - 3*b*xa^3 - 2*b*xb^3 - 14*k1*xa^2 - 16*k2*xa^2 + 16*k1*xb^2 + 14*k2*xb^2 - 4*a*xa*xb^2 - a*xa^2*xb + b*xa*xb^2 + 4*b*xa^2*xb - 2*k1*xa*xb + 2*k2*xa*xb)/(2*(xa - xb)*(xa^4 - 4*xa^3*xb + 6*xa^2*xb^2 - 4*xa*xb^3 + xb^4));
a5 = (12*ya - 12*yb - 6*k1*xa - 6*k2*xa + 6*k1*xb + 6*k2*xb + a*xa^2 + a*xb^2 - b*xa^2 - b*xb^2 - 2*a*xa*xb + 2*b*xa*xb)/(2*(xa - xb)^2*(xa^3 - 3*xa^2*xb + 3*xa*xb^2 - xb^3));

plan_x=zeros(1,300);
plan_y=zeros(1,300);
dydx=zeros(1,300);
dydx2=zeros(1,300);
pm_cur=single(zeros(1,300));


for i=uint16(1):uint16(300)
    if i==uint16(1)
        plan_x(i)=xa;
    else
        dx = cos(atan(dydx(i)))*0.05;
        plan_x(i)=plan_x(i-1)+dx;
    end

    x=plan_x(i);
    plan_y(i)=a0+a1*x+a2*x^2+a3*x^3+a4*x^4+a5*x^5;
    dydx(i)=a1 + 2*a2*x + 3*a3*x^2 + 4*a4*x^3 + 5*a5*x^4;
    dydx2(i)=2*a2 + 3*2*a3*x + 4*3*a4*x^2 + 5*4*a5*x^3;
    pm_cur(i)=single(dydx2(i)/((1+dydx(i)^2)^1.5));

    Dst2End=((plan_x(i) - end_x)^2+ (plan_y(i)-end_y)^2)^0.5;
    Dst2End=abs(Dst2End);
    if Dst2End < 0.06  %------%到达路径终点
        break
    end
end
rad_yaw=atan(dydx);
Points_Num=i;
mm_plan_x=1000*plan_x;%------单位 m >>>>> mm
mm_plan_y=1000*plan_y;%------单位 m >>>>> mm
end

